﻿using System;
using System.Collections.Generic;
using System.Text;
using SharpPTCLib;
using System.Drawing;

namespace SharpPTCSamples
{
    class VerletRope
    {
        const float AIR_FRICTION = 0.99f;
        const int CONSTRAINT_ITERATIONS = 10;
        const float GRAVITY = 0.1f;
        static List<Constraint> constraintList = new List<Constraint>();
        static List<PointMass> pointMassList = new List<PointMass>();
        static SharpPTC sd;
        static PointMass pFirst;
        static PointMass pEnd;

        static void Main(string[] args)
        {
            Random rand = new Random();
            sd = new SharpPTC();
            sd.StartAll("Verlet integration demo", 320, 240, 2, false);
            sd.SetClsColor(Color.Black);

            PointMass p1 = new PointMass(20, 50);
            pointMassList.Add(p1);
            pFirst = p1;

            for (int i = 1; i < 10; i++)
            {
                PointMass p2 = new PointMass(20 + (i * 7), 50, 0, -10);
                pointMassList.Add(p2);
                Constraint c = new Constraint(p1, p2);
                constraintList.Add(c);
                p1 = p2;
            }
            pEnd = p1;

            //Create a box
            PointMass bp1 = new PointMass(100, 100, 10, 0);
            PointMass bp2 = new PointMass(120, 100, 0, 0);
            PointMass bp3 = new PointMass(100, 120, 0, 0);
            PointMass bp4 = new PointMass(120, 120, 0, -10);
            pointMassList.Add(bp1);
            pointMassList.Add(bp2);
            pointMassList.Add(bp3);
            pointMassList.Add(bp4);

            constraintList.Add(new Constraint(bp1, bp2));
            constraintList.Add(new Constraint(bp2, bp4));
            constraintList.Add(new Constraint(bp4, bp3));
            constraintList.Add(new Constraint(bp3, bp1));
            constraintList.Add(new Constraint(bp3, bp2));
            constraintList.Add(new Constraint(bp1, bp4));

            do
            {
                sd.Cls();

                //Steer rope
                if (sd.IsKeyDown(Scancode.LEFT))
                    pEnd.X--;
                if (sd.IsKeyDown(Scancode.RIGHT))
                    pEnd.X++;
                if (sd.IsKeyDown(Scancode.UP))
                    pEnd.Y--;
                if (sd.IsKeyDown(Scancode.DOWN))
                    pEnd.Y++;

                //Release/Secure rope
                if (sd.IsKeyPressed(Scancode.SPACE))
                    pFirst.Secured = !pFirst.Secured;

                //draw ground
                sd.SetDrawColor(Color.ForestGreen);
                sd.DrawRectangle(0, 201, 319, 47);

                UpdatePointMasses();
                UpdateConstraints();
                DrawConstraints();
            } while (sd.UpdateAll() && !sd.IsKeyDown(Scancode.ESCAPE));
        }

        /// <summary>
        /// Draws lines between constraints
        /// </summary>
        static void DrawConstraints()
        {
            sd.SetDrawColor(Color.Goldenrod);
            foreach (Constraint c in constraintList)
                sd.DrawLine((int)c.P1.X, (int)c.P1.Y, (int)c.P2.X, (int)c.P2.Y);
        }

        /// <summary>
        /// Updates all masses
        /// </summary>
        static void UpdatePointMasses()
        {
            foreach (PointMass p in pointMassList)
            {
                float dx = p.X - p.oX;
                float dy = p.Y - p.oY + (GRAVITY);

                if (!p.Secured)
                {
                    p.oX = p.X;
                    p.oY = p.Y;
                    p.X += dx * AIR_FRICTION;
                    p.Y += dy * AIR_FRICTION;
                }

                //Simulate ground
                if (p.Y > 200)
                {
                    p.Y = 200;
                    dx = p.X - p.oX;
                    dx = dx * 0.5f;
                    p.oX = p.oX + dx;
                }
            }
        }

        /// <summary>
        /// Updates constraints
        /// </summary>
        static void UpdateConstraints()
        {
            for (int i = 0; i < CONSTRAINT_ITERATIONS; i++)
            {
                foreach (Constraint c in constraintList)
                {
                    float dx = c.P1.X - c.P2.X;
                    float dy = c.P1.Y - c.P2.Y;
                    float dist = (float)Math.Sqrt((dx * dx) + (dy * dy));
                    float diff = dist - c.Length;

                    if (c.Length != 0)
                        diff = diff / c.Length;
                    else
                        diff = 0;

                    dx = dx * c.Stiffness;
                    dy = dy * c.Stiffness;

                    if (!c.P1.Secured)
                    {
                        c.P1.X -= (diff * dx);
                        c.P1.Y -= (diff * dy);
                    }
                    if (!c.P2.Secured)
                    {
                        c.P2.X += (diff * dx);
                        c.P2.Y += (diff * dy);
                    }
                }
            }
        }

        class PointMass
        {
            public float X, Y, oX, oY, Mass;
            public bool Secured = false;

            public PointMass(float x, float y, float vx, float vy)
            {
                X = x;
                Y = y;
                oX = x - vx;
                oY = y - vy;
                Mass = 1;
            }

            public PointMass(float x, float y)
            {
                X = oX = x;
                Y = oY = y;
                Secured = true;
            }
        }

        class Constraint
        {
            public PointMass P1, P2;
            public float Length;
            public float Stiffness = 0.5f;

            public Constraint(PointMass p1, PointMass p2)
            {
                P1 = p1;
                P2 = p2;
                Length = (float)Math.Sqrt(((p1.X - p2.X) * (p1.X - p2.X)) + ((p1.Y - p2.Y) * (p1.Y - p2.Y)));
            }

            public Constraint(PointMass p1, PointMass p2, float stiffness) : this(p1, p2)
            {
                Stiffness = stiffness;
            }
        }
    }
}
